#pragma once

#include <stdio.h>

#include "visited_list_pool.h"
#include "hnswlib.h"
#include <random>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <algorithm>
#include <atomic>
#include <unordered_set>
#include <unordered_map>


namespace hnswlib {
    typedef unsigned int tableint;
    typedef unsigned int linklistsizeint;

    template<typename dist_t>
    class HierarchicalNSW : public AlgorithmInterface<dist_t> {
    public:

        HierarchicalNSW(SpaceInterface<dist_t> *s) {

        }

        HierarchicalNSW(SpaceInterface<dist_t> *s, const string &location, bool nmslib = false) {
            loadIndex(location, s);
        }

        // SpaceInterface度量空间类，max_elements最大索引数目，M
        HierarchicalNSW(SpaceInterface<dist_t> *s, size_t max_elements, size_t M = 16, size_t ef_construction = 200) :
                link_list_locks_(max_elements), element_levels_(max_elements) { // 边链表大小初始化为最大向量数目，每个向量所在层数
            max_elements_ = max_elements; // 最大索引数据量

            data_size_ = s->get_data_size(); // 一个向量所占字节数
            fstdistfunc_ = s->get_dist_func(); // 计算距离函数
            dist_func_param_ = s->get_dist_func_param(); // 获取距离计算函数的参数：向量维度
            M_ = M;
            maxM_ = M_; // 第i层每个节点最大连接数目？
            maxM0_ = M_ * 2; // 第0层每个节点最大连接数目？
            ef_construction_ = std::max(ef_construction, M_);
            ef_ = 10;

            size_links_level0_ = maxM0_ * sizeof(tableint) + sizeof(linklistsizeint); // 260, 第0层连接数目
            size_data_per_element_ = size_links_level0_ + data_size_ + sizeof(labeltype); // 260+512+8=780, 每一个向量占用字节数？
            offsetData_ = size_links_level0_; // 260, 偏移量, data起始的位置
            label_offset_ = size_links_level0_ + data_size_; // 260+512=772, 偏移量, label起始的位置
            offsetLevel0_ = 0; // 层次0偏移量

            data_level0_memory_ = (char *) malloc(max_elements_ * size_data_per_element_); // 所有向量在第0层占用的总字节数, max_elements_*780(260+512+8)(边数目+数据+label)
            if (data_level0_memory_ == nullptr) // 异常处理
                throw new std::runtime_error("Not enough memory");

            cur_element_count = 0; // 当前节点的链接数目，最大边数目64？

            visited_list_pool_ = new VisitedListPool(1, (int)max_elements); // 初始化initmaxpools为1，numelements初始化为所有向量数目

            //initializations for special treatment of the first node
            enterpoint_node_ = -1; // 输入节点数目
            maxlevel_ = -1; // 最大层数目

            // 调试信息
            printf("void * size: %d\n", sizeof(void *));
                    
            linkLists_ = (char **) malloc(sizeof(void *) * max_elements_); // 35185个边链表列表
            size_links_per_element_ = maxM_ * sizeof(tableint) + sizeof(linklistsizeint); // 每一个向量连接占用的字节数：32*4 + 4
            mult_ = 1 / log(1.0 * M_); // a simple chioce for the optimal levelMult is 1/ln(M)
            revSize_ = 1.0 / mult_;
        }

        struct CompareByFirst {
            constexpr bool operator()(pair<dist_t, tableint> const &a,
                                      pair<dist_t, tableint> const &b) const noexcept {
                return a.first < b.first;
            }
        };

        ~HierarchicalNSW() {

            free(data_level0_memory_);
            for (tableint i = 0; i < cur_element_count; i++) {
                if (element_levels_[i] > 0)
                    free(linkLists_[i]);
            }
            free(linkLists_);
            delete visited_list_pool_;
        }

        size_t max_elements_; // 向量最大数目
        size_t cur_element_count; // 当前已有向量数目
        size_t size_data_per_element_; // 每一个向量所占字节数目
        size_t size_links_per_element_; // 每个向量作为顶点，对应边数目大小

        size_t M_;
        size_t maxM_;
        size_t maxM0_;
        size_t ef_construction_;

        double mult_, revSize_;
        int maxlevel_;


        VisitedListPool *visited_list_pool_;
        mutex cur_element_count_guard_;

        vector<mutex> link_list_locks_; // 互斥量
        tableint enterpoint_node_;



        size_t size_links_level0_;
        size_t offsetData_, offsetLevel0_;


        char *data_level0_memory_;
        char **linkLists_;
        vector<int> element_levels_;


        size_t data_size_;
        size_t label_offset_;
        DISTFUNC<dist_t> fstdistfunc_;
        void *dist_func_param_;

        std::default_random_engine level_generator_ = std::default_random_engine(100);

        inline labeltype getExternalLabel(tableint internal_id) {
            return *((labeltype *) (data_level0_memory_ + internal_id * size_data_per_element_ + label_offset_));
        }

        inline labeltype *getExternalLabeLp(tableint internal_id) {
            return (labeltype *) (data_level0_memory_ + internal_id * size_data_per_element_ + label_offset_);
        }

        inline char *getDataByInternalId(tableint internal_id) {
            return (data_level0_memory_ + internal_id * size_data_per_element_ + offsetData_);
        }

        // select a random level with an exponential distribuion
        int getRandomLevel(double reverse_size) {
            std::uniform_real_distribution<double> distribution(0.0, 1.0);
            double r = -log(distribution(level_generator_)) * reverse_size;
            return (int) r;
        }

        // SELECT-NEIGHBORS, 算法3或者算法4
        std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst>
        searchBaseLayer(tableint enterpoint_id, void *data_point, int layer) {
            VisitedList *vl = visited_list_pool_->getFreeVisitedList();
            vl_type *visited_array = vl->mass;
            vl_type visited_array_tag = vl->curV;

            std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> top_candidates;
            std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> candidateSet;
            dist_t dist = fstdistfunc_(data_point, getDataByInternalId(enterpoint_id), dist_func_param_);

            top_candidates.emplace(dist, enterpoint_id);
            candidateSet.emplace(-dist, enterpoint_id);
            visited_array[enterpoint_id] = visited_array_tag;
            dist_t lowerBound = dist;

            while (!candidateSet.empty()) {

                std::pair<dist_t, tableint> curr_el_pair = candidateSet.top();

                if ((-curr_el_pair.first) > lowerBound) {
                    break;
                }
                candidateSet.pop();

                tableint curNodeNum = curr_el_pair.second;

                unique_lock <mutex> lock(link_list_locks_[curNodeNum]);

                int *data;// = (int *)(linkList0_ + curNodeNum * size_links_per_element0_);
                if (layer == 0)
                    data = (int *) (data_level0_memory_ + curNodeNum * size_data_per_element_ + offsetLevel0_);
                else
                    data = (int *) (linkLists_[curNodeNum] + (layer - 1) * size_links_per_element_);
                int size = *data;
                tableint *datal = (tableint *) (data + 1);
                _mm_prefetch((char *) (visited_array + *(data + 1)), _MM_HINT_T0);
                _mm_prefetch((char *) (visited_array + *(data + 1) + 64), _MM_HINT_T0);
                _mm_prefetch(getDataByInternalId(*datal), _MM_HINT_T0);
                _mm_prefetch(getDataByInternalId(*(datal + 1)), _MM_HINT_T0);

                for (int j = 0; j < size; j++) {
                    tableint candidate_id = *(datal + j);
                    _mm_prefetch((char *) (visited_array + *(datal + j + 1)), _MM_HINT_T0);
                    _mm_prefetch(getDataByInternalId(*(datal + j + 1)), _MM_HINT_T0);
                    if (visited_array[candidate_id] == visited_array_tag) continue;
                    visited_array[candidate_id] = visited_array_tag;
                    char *currObj1 = (getDataByInternalId(candidate_id));

                    dist_t dist1 = fstdistfunc_(data_point, currObj1, dist_func_param_);
                    if (top_candidates.top().first > dist1 || top_candidates.size() < ef_construction_) {
                        candidateSet.emplace(-dist1, candidate_id);
                        _mm_prefetch(getDataByInternalId(candidateSet.top().second), _MM_HINT_T0);
                        top_candidates.emplace(dist1, candidate_id);
                        if (top_candidates.size() > ef_construction_) {
                            top_candidates.pop();
                        }
                        lowerBound = top_candidates.top().first;
                    }
                }
            }
            visited_list_pool_->releaseVisitedList(vl);

            return top_candidates;
        }

        std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst>
        searchBaseLayerST(tableint ep_id, void *data_point, size_t ef) {
            VisitedList *vl = visited_list_pool_->getFreeVisitedList();
            vl_type *visited_array = vl->mass;
            vl_type visited_array_tag = vl->curV;

            std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> top_candidates;
            std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> candidate_set;
            dist_t dist = fstdistfunc_(data_point, getDataByInternalId(ep_id), dist_func_param_);

            top_candidates.emplace(dist, ep_id);
            candidate_set.emplace(-dist, ep_id);
            visited_array[ep_id] = visited_array_tag;
            dist_t lower_bound = dist;

            while (!candidate_set.empty()) {

                std::pair<dist_t, tableint> current_node_pair = candidate_set.top();

                if ((-current_node_pair.first) > lower_bound) {
                    break;
                }
                candidate_set.pop();

                tableint current_node_id = current_node_pair.second;
                int *data = (int *) (data_level0_memory_ + current_node_id * size_data_per_element_ + offsetLevel0_);
                int size = *data;
                _mm_prefetch((char *) (visited_array + *(data + 1)), _MM_HINT_T0);
                _mm_prefetch((char *) (visited_array + *(data + 1) + 64), _MM_HINT_T0);
                _mm_prefetch(data_level0_memory_ + (*(data + 1)) * size_data_per_element_ + offsetData_, _MM_HINT_T0);
                _mm_prefetch((char *) (data + 2), _MM_HINT_T0);

                for (int j = 1; j <= size; j++) {
                    int candidate_id = *(data + j);
                    _mm_prefetch((char *) (visited_array + *(data + j + 1)), _MM_HINT_T0);
                    _mm_prefetch(data_level0_memory_ + (*(data + j + 1)) * size_data_per_element_ + offsetData_,
                                 _MM_HINT_T0);////////////
                    if (!(visited_array[candidate_id] == visited_array_tag)) {

                        visited_array[candidate_id] = visited_array_tag;

                        char *currObj1 = (getDataByInternalId(candidate_id));
                        dist_t dist = fstdistfunc_(data_point, currObj1, dist_func_param_);

                        if (top_candidates.top().first > dist || top_candidates.size() < ef) {
                            candidate_set.emplace(-dist, candidate_id);
                            _mm_prefetch(data_level0_memory_ + candidate_set.top().second * size_data_per_element_ +
                                         offsetLevel0_,///////////
                                         _MM_HINT_T0);////////////////////////

                            top_candidates.emplace(dist, candidate_id);

                            if (top_candidates.size() > ef) {
                                top_candidates.pop();
                            }
                            lower_bound = top_candidates.top().first;
                        }
                    }
                }
            }

            visited_list_pool_->releaseVisitedList(vl);
            return top_candidates;
        }

        // 对应论文SELECT-NEIGHBORS-HEURISTIC部分
        void getNeighborsByHeuristic2(
                std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> &top_candidates,
                const int M) {
            if (top_candidates.size() < M) {
                return;
            }
            std::priority_queue<std::pair<dist_t, tableint>> queue_closest;
            vector<std::pair<dist_t, tableint>> return_list;
            while (top_candidates.size() > 0) {
                queue_closest.emplace(-top_candidates.top().first, top_candidates.top().second);
                top_candidates.pop();
            }

            while (queue_closest.size()) {
                if (return_list.size() >= M)
                    break;
                std::pair<dist_t, tableint> curent_pair = queue_closest.top();
                dist_t dist_to_query = -curent_pair.first;
                queue_closest.pop();
                bool good = true;
                for (std::pair<dist_t, tableint> second_pair : return_list) {
                    dist_t curdist =
                            fstdistfunc_(getDataByInternalId(second_pair.second),
                                         getDataByInternalId(curent_pair.second),
                                         dist_func_param_);;
                    if (curdist < dist_to_query) {
                        good = false;
                        break;
                    }
                }
                if (good) {
                    return_list.push_back(curent_pair);
                }


            }

            for (std::pair<dist_t, tableint> curent_pair : return_list) {

                top_candidates.emplace(-curent_pair.first, curent_pair.second);
            }
        }


        linklistsizeint *get_linklist0(tableint internal_id) {
            return (linklistsizeint *) (data_level0_memory_ + internal_id * size_data_per_element_ + offsetLevel0_);
        };

        linklistsizeint *get_linklist0(tableint internal_id, char *data_level0_memory_) {
            return (linklistsizeint *) (data_level0_memory_ + internal_id * size_data_per_element_ + offsetLevel0_);
        };

        linklistsizeint *get_linklist(tableint internal_id, int level) {
            return (linklistsizeint *) (linkLists_[internal_id] + (level - 1) * size_links_per_element_);
        };

        // 算法1第12行
        void mutuallyConnectNewElement(void *data_point, tableint cur_c,
                                       std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> top_candidates,
                                       int level) {

            size_t Mcurmax = level ? maxM_ : maxM0_; // 32 or 64
            getNeighborsByHeuristic2(top_candidates, M_);
            if (top_candidates.size() > M_) // 最近邻返回的数目，不能超过设置的M
                throw runtime_error("Should be not be more than M_ candidates returned by the heuristic");

            vector<tableint> selectedNeighbors;
            selectedNeighbors.reserve(M_); // 保存候选的近邻
            while (top_candidates.size() > 0) {
                selectedNeighbors.push_back(top_candidates.top().second);
                top_candidates.pop();
            }
            {
                linklistsizeint *ll_cur;
                if (level == 0)
                    ll_cur = get_linklist0(cur_c); // 获取当前向量的链表
                else
                    ll_cur = get_linklist(cur_c, level);

                if (*ll_cur) {
                    cout << *ll_cur << "\n";
                    cout << element_levels_[cur_c] << "\n";
                    cout << level << "\n";
                    throw runtime_error("The newly inserted element should have blank link list");
                }
                *ll_cur = selectedNeighbors.size();
                tableint *data = (tableint *) (ll_cur + 1);


                for (int idx = 0; idx < selectedNeighbors.size(); idx++) {
                    if (data[idx])
                        throw runtime_error("Possible memory corruption");
                    if (level > element_levels_[selectedNeighbors[idx]])
                        throw runtime_error("Trying to make a link on a non-existent level");

                    data[idx] = selectedNeighbors[idx];

                }
            }
            
            // 算法1第13行
            for (int idx = 0; idx < selectedNeighbors.size(); idx++) {

                unique_lock <mutex> lock(link_list_locks_[selectedNeighbors[idx]]);
                linklistsizeint *ll_other;
                if (level == 0)
                    ll_other = get_linklist0(selectedNeighbors[idx]); // 获取链表
                else
                    ll_other = get_linklist(selectedNeighbors[idx], level);
                int sz_link_list_other = *ll_other; // 获取链表大小


                if (sz_link_list_other > Mcurmax || sz_link_list_other < 0)
                    throw runtime_error("Bad value of sz_link_list_other");
                if (selectedNeighbors[idx] == cur_c)
                    throw runtime_error("Trying to connect an element to itself");
                if (level > element_levels_[selectedNeighbors[idx]])
                    throw runtime_error("Trying to make a link on a non-existent level");

                tableint *data = (tableint *) (ll_other + 1);
                if (sz_link_list_other < Mcurmax) {
                    data[sz_link_list_other] = cur_c;
                    *ll_other = sz_link_list_other + 1;
                } else {
                    // finding the "weakest" element to replace it with the new one
                    dist_t d_max = fstdistfunc_(getDataByInternalId(cur_c), getDataByInternalId(selectedNeighbors[idx]),
                                                dist_func_param_);
                    // Heuristic:
                    std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> candidates;
                    candidates.emplace(d_max, cur_c);

                    for (int j = 0; j < sz_link_list_other; j++) {
                        candidates.emplace(
                                fstdistfunc_(getDataByInternalId(data[j]), getDataByInternalId(selectedNeighbors[idx]),
                                             dist_func_param_), data[j]);
                    }

                    getNeighborsByHeuristic2(candidates, Mcurmax);

                    int indx = 0;
                    while (candidates.size() > 0) {
                        data[indx] = candidates.top().second;
                        candidates.pop();
                        indx++;
                    }
                    *ll_other = indx;
                    // Nearest K:
                    /*int indx = -1;
                    for (int j = 0; j < sz_link_list_other; j++) {
                        dist_t d = fstdistfunc_(getDataByInternalId(data[j]), getDataByInternalId(rez[idx]), dist_func_param_);
                        if (d > d_max) {
                            indx = j;
                            d_max = d;
                        }
                    }
                    if (indx >= 0) {
                        data[indx] = cur_c;
                    } */
                }

            }
        }

        mutex global;
        size_t ef_;

        void setEf(size_t ef) {
            ef_ = ef;
        }


        std::priority_queue<std::pair<dist_t, tableint>> searchKnnInternal(void *query_data, int k) {
            tableint currObj = enterpoint_node_;
            dist_t curdist = fstdistfunc_(query_data, getDataByInternalId(enterpoint_node_), dist_func_param_);

            for (int level = maxlevel_; level > 0; level--) {
                bool changed = true;
                while (changed) {
                    changed = false;
                    int *data;
                    data = (int *) (linkLists_[currObj] + (level - 1) * size_links_per_element_);
                    int size = *data;
                    tableint *datal = (tableint *) (data + 1);
                    for (int i = 0; i < size; i++) {
                        tableint cand = datal[i];
                        if (cand < 0 || cand > max_elements_)
                            throw runtime_error("cand error");
                        dist_t d = fstdistfunc_(query_data, getDataByInternalId(cand), dist_func_param_);

                        if (d < curdist) {
                            curdist = d;
                            currObj = cand;
                            changed = true;
                        }
                    }
                }
            }

            //std::priority_queue< std::pair< dist_t, tableint  >> top_candidates = searchBaseLayer(currObj, query_data, 0);
            std::priority_queue<std::pair<dist_t, tableint  >> top_candidates = searchBaseLayerST(currObj, query_data,
                                                                                                  ef_);
            while (top_candidates.size() > k) {
                top_candidates.pop();
            }
            return top_candidates;
        };

        // 保存索引
        void saveIndex(const string &location) {
            std::ofstream output(location, std::ios::binary);
            streampos position;

            writeBinaryPOD(output, offsetLevel0_); // 0
            writeBinaryPOD(output, max_elements_); // 最大索引量
            writeBinaryPOD(output, cur_element_count); // 索引到的向量数目
            writeBinaryPOD(output, size_data_per_element_); // 780, 每个向量所占的字节数目
            writeBinaryPOD(output, label_offset_); // 772
            writeBinaryPOD(output, offsetData_); // 260
            writeBinaryPOD(output, maxlevel_); //最大层数2
            writeBinaryPOD(output, enterpoint_node_); // 入节点
            writeBinaryPOD(output, maxM_); // 每个节点的边最大数目

            writeBinaryPOD(output, maxM0_); // 第0层每个节点的边最大数目
            writeBinaryPOD(output, M_); // 32
            writeBinaryPOD(output, mult_);
            writeBinaryPOD(output, ef_construction_); // 80

            output.write(data_level0_memory_, max_elements_ * size_data_per_element_); // 最大索引量所占字节数

            for (size_t i = 0; i < max_elements_; i++) {
                unsigned int linkListSize = element_levels_[i] > 0 ? size_links_per_element_ * element_levels_[i] : 0;
                writeBinaryPOD(output, linkListSize);
                if (linkListSize)
                    output.write(linkLists_[i], linkListSize);
            }
            output.close();
        }

        // 载入索引
        void loadIndex(const string &location, SpaceInterface<dist_t> *s) {


            //cout << "Loading index from " << location;
            std::ifstream input(location, std::ios::binary);
            streampos position;

            readBinaryPOD(input, offsetLevel0_);
            readBinaryPOD(input, max_elements_);
            readBinaryPOD(input, cur_element_count);
            readBinaryPOD(input, size_data_per_element_);
            readBinaryPOD(input, label_offset_);
            readBinaryPOD(input, offsetData_);
            readBinaryPOD(input, maxlevel_);
            readBinaryPOD(input, enterpoint_node_);

            readBinaryPOD(input, maxM_);
            readBinaryPOD(input, maxM0_);
            readBinaryPOD(input, M_);
            readBinaryPOD(input, mult_);
            readBinaryPOD(input, ef_construction_);


            data_size_ = s->get_data_size();
            fstdistfunc_ = s->get_dist_func();
            dist_func_param_ = s->get_dist_func_param();

            data_level0_memory_ = (char *) malloc(max_elements_ * size_data_per_element_);
            input.read(data_level0_memory_, max_elements_ * size_data_per_element_);


            size_links_per_element_ = maxM_ * sizeof(tableint) + sizeof(linklistsizeint);


            size_links_level0_ = maxM0_ * sizeof(tableint) + sizeof(linklistsizeint);
            vector<mutex>(max_elements_).swap(link_list_locks_);

            visited_list_pool_ = new VisitedListPool(1, (int)max_elements_);

            linkLists_ = (char **) malloc(sizeof(void *) * max_elements_);
            element_levels_ = vector<int>(max_elements_);
            revSize_ = 1.0 / mult_;
            ef_ = 10;
            for (size_t i = 0; i < max_elements_; i++) {
                unsigned int linkListSize;
                readBinaryPOD(input, linkListSize);
                if (linkListSize == 0) {
                    element_levels_[i] = 0;

                    linkLists_[i] = nullptr;
                } else {
                    element_levels_[i] = linkListSize / size_links_per_element_;
                    linkLists_[i] = (char *) malloc(linkListSize);
                    input.read(linkLists_[i], linkListSize);
                }
            }
            input.close();

            return;
        }

        // data_point为插入数据，label为第几个向量
        void addPoint(void *data_point, labeltype label)
        {
            addPoint(data_point, label, -1);
        };

        // tableint: unsigned int, labeltype: size_t
        tableint addPoint(void *data_point, labeltype label, int level) // level = -1
        {
            tableint cur_c = 0; // 当前表初始化？当前计数器？
            {
                unique_lock <mutex> lock(cur_element_count_guard_); // std::unique_lock 与std::lock_guard都能实现自动加锁与解锁功能，但是std::unique_lock要比std::lock_guard更灵活，但是更灵活的代价是占用空间相对更大一点且相对更慢一点。https://blog.csdn.net/tgxallen/article/details/73522233
                if (cur_element_count >= max_elements_) { // 当前向量数目计数器
                    cout << "The number of elements exceeds the specified limit\n";
                    throw runtime_error("The number of elements exceeds the specified limit");
                };
                cur_c = (tableint)cur_element_count;
                cur_element_count++;
            }
            unique_lock <mutex> lock_el(link_list_locks_[cur_c]); // 当前边链表上锁(边链表初始化为最大向量数目)
            int curlevel = getRandomLevel(mult_); // 随机生成层数
            if (level > 0)
                curlevel = level;

            element_levels_[cur_c] = curlevel; // 当前向量所在层数


            unique_lock <mutex> templock(global); // 临时锁
            int maxlevelcopy = maxlevel_;
            if (curlevel <= maxlevelcopy)
                templock.unlock();
            tableint currObj = enterpoint_node_;


            // offsetLevel0_, 0层偏移量
            memset(data_level0_memory_ + cur_c * size_data_per_element_ + offsetLevel0_, 0, size_data_per_element_); // 将s中当前位置后面的n个字节(typedef unsigned int size_t)用ch替换并返回s

            // Initialisation of the data and label
            memcpy(getExternalLabeLp(cur_c), &label, sizeof(labeltype));
            memcpy(getDataByInternalId(cur_c), data_point, data_size_);


            if (curlevel) {
                linkLists_[cur_c] = (char *) malloc(size_links_per_element_ * curlevel);
                memset(linkLists_[cur_c], 0, size_links_per_element_ * curlevel);
            }
            if (currObj != -1) {


                if (curlevel < maxlevelcopy) {

                    dist_t curdist = fstdistfunc_(data_point, getDataByInternalId(currObj), dist_func_param_);
                    for (int level = maxlevelcopy; level > curlevel; level--) {


                        bool changed = true;
                        while (changed) {
                            changed = false;
                            int *data;
                            unique_lock <mutex> lock(link_list_locks_[currObj]);
                            data = (int *) (linkLists_[currObj] + (level - 1) * size_links_per_element_);
                            int size = *data;
                            tableint *datal = (tableint *) (data + 1);
                            for (int i = 0; i < size; i++) {
                                tableint cand = datal[i];
                                if (cand < 0 || cand > max_elements_)
                                    throw runtime_error("cand error");
                                dist_t d = fstdistfunc_(data_point, getDataByInternalId(cand), dist_func_param_);
                                if (d < curdist) {
                                    curdist = d;
                                    currObj = cand;
                                    changed = true;
                                }
                            }
                        }
                    }
                }

                // 算法1第9行
                for (int level = min(curlevel, maxlevelcopy); level >= 0; level--) {
                    if (level > maxlevelcopy || level < 0)
                        throw runtime_error("Level error");

                    // SELECT-NEIGHBORS, 算法3或者算法4
                    std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> top_candidates = searchBaseLayer(currObj, data_point, level);
                    mutuallyConnectNewElement(data_point, cur_c, top_candidates, level);
                }


            } else {
                // Do nothing for the first element,对于第一个向量，不做任何操作
                enterpoint_node_ = 0;
                maxlevel_ = curlevel;

            }

            //Releasing lock for the maximum level
            if (curlevel > maxlevelcopy) {
                enterpoint_node_ = cur_c;
                maxlevel_ = curlevel;
            }
            return cur_c;
        };

        // 搜索接口
        std::priority_queue<std::pair<dist_t, labeltype >> searchKnn(void *query_data, size_t k) {
            tableint currObj = enterpoint_node_;
            dist_t curdist = fstdistfunc_(query_data, getDataByInternalId(enterpoint_node_), dist_func_param_);

            for (int level = maxlevel_; level > 0; level--) {
                bool changed = true;
                while (changed) {
                    changed = false;
                    int *data;
                    data = (int *) (linkLists_[currObj] + (level - 1) * size_links_per_element_);
                    int size = *data;
                    tableint *datal = (tableint *) (data + 1);
                    for (int i = 0; i < size; i++) {
                        tableint cand = datal[i];
                        if (cand < 0 || cand > max_elements_)
                            throw runtime_error("cand error");
                        dist_t d = fstdistfunc_(query_data, getDataByInternalId(cand), dist_func_param_);

                        if (d < curdist) {
                            curdist = d;
                            currObj = cand;
                            changed = true;
                        }
                    }
                }
            }


            std::priority_queue<std::pair<dist_t, tableint>, vector<pair<dist_t, tableint>>, CompareByFirst> top_candidates = searchBaseLayerST(
                    currObj, query_data, std::max(ef_,k));
            std::priority_queue<std::pair<dist_t, labeltype >> results;
            while (top_candidates.size() > k) {
                top_candidates.pop();
            }
            while (top_candidates.size() > 0) {
                std::pair<dist_t, tableint> rez = top_candidates.top();
                results.push(std::pair<dist_t, labeltype>(rez.first, getExternalLabel(rez.second)));
                top_candidates.pop();
            }
            return results;
        };


    };

}
